/* clock em Hz */
#define F_CPU 16000000UL

#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include "rot_tsetups.h"
#include "rot_servo.h"
#include "rot_lv_serial.h"
#include "rot_adc.h"
#include "rot_tacho.h"
#include "rot_ap.h"

#define MANUAL 0
#define AUTO 1

#define MODE 0
#define FORCA 1
#define CMD 2
#define RPM 3
#define RPM_D 4
#define LAB 5

void check_mode(uint8_t *mode) {

  static int last_ppm;
  int mode_ppm = ppm_vals[MODE_CH];

  if ( (last_ppm>3000) && (mode_ppm>3000) ) {
    *mode = AUTO;
  } else {
    *mode = MANUAL;
  }
  
  last_ppm = mode_ppm;

}

void get_data(int* data_vec) {

  data_vec[FORCA] = adc_read(PC0);
  data_vec[CMD] = ppm_vals[MOTOR_CH];
  data_vec[RPM] = tacho_get_rpm();
  data_vec[RPM_D] = pwm_to_rpm(ppm_vals[MOTOR_CH]);
  data_vec[LAB] = labview_pos;

}

void print_fmt_data(int *data) {
 
  char msg[100];
  
  sprintf(msg,"!M%d,P%d,LP%d,F%d,R%d,RD%d\n",data[MODE],
	  data[CMD],data[LAB],data[FORCA],data[RPM],data[RPM_D]);
  serial_transmit(msg);

}

void main_init(void) {

  timer1_init();
  timer2_init();
  serial_init((F_CPU/SERIAL_BAUD/16)-1);
  servo_init();
  adc_init();
  tacho_init();
  //ap_init();

}

int main(void) {

  uint8_t cur_mode=MANUAL;
  int pos;
  int data[6];

  main_init();
  /* Enable Interrupts */
  sei();
  
  while (1){
    if (TASK1_FLAG) {
      check_mode(&cur_mode);
      if (cur_mode == MANUAL) {
	pos = ppm_vals[MOTOR_CH];
	if (soma) soma = 0;
	MOVE_SERVO_TO(pos);
      } else if (cur_mode == AUTO) {
	//run_ap();
	//pos = ctl_pos;
	get_pos_from_labview();
	pos = labview_pos;
	MOVE_SERVO_TO(pos);
      } else {
	pos = SERVO_MIN;
	MOVE_SERVO_TO(pos);
      }
      TASK1_FLAG = 0;
    }
    if (TASK2_FLAG) {
      data[MODE] = (int) cur_mode;
      get_data(data);
      print_fmt_data(data);
      TASK2_FLAG = 0;
    }
  }
  return 0;
};
